/**
 ******************************************************************************
 * @FilePath: \Hero\Module\Shoot.c
 * @Brief: 
 * @Date: 2021-01-12 15:55:46
 * @Author: Rio
 ******************************************************************************
 */
#include "Shoot.h"
#include "remoteCtrl.h"
#include "pid.h"
#include "gpio.h"
#include "Motor_control.h"
#include "bsp_can.h"
#include <string.h>
#include <math.h>
#include "STMGood.h"
#include "Mode_SW.h"
#include "Gimbal.h"
#include "bsp_timer.h"
/*********************************** Private defines *****************************************/
#define S_LOW_FREQUENCY 1000
#define S_MID_FREQUENCY 800
#define S_HIGH_FREQUENCY 500

#define STEP 360.0*19.203208556/6
#define BULL_HEAT 100
/***********************PID para*****************************/
#define TRANS_MOTOR_SPEED_PID_KP                          20
#define TRANS_MOTOR_SPEED_PID_KI                           0
#define TRANS_MOTOR_SPEED_PID_KD                           0
#define TRANS_MOTOR_SPEED_PID_ERRILIM                      0
#define TRANS_MOTOR_SPEED_PID_MAXOUT                   15000

#define TRANS_MOTOR_POS_PID_KP                             8
#define TRANS_MOTOR_POS_PID_KI                             0
#define TRANS_MOTOR_POS_PID_KD                             0
#define TRANS_MOTOR_POS_PID_ERRILIM                        0
#define TRANS_MOTOR_POS_PID_MAXOUT                      1200

/********************************** Extern variables ****************************************/
extern remoteCtrl_t s_rcData;
extern float        p,i,d,S1;
extern float        P,I,D,S2;
/********************************** Private variables ****************************************/
static int16_t shootFreq = S_LOW_FREQUENCY; // Low shooting frequency
static int16_t                       count;
static float                    transOut;
static int16_t    friSped_l, friSped_r = 0;

uint8_t 			can2_txBuff[8] = {1};
uint8_t         shootMode = S_STOP;
motorData_t    s_trans_motor = {0};

PID_AbsoluteType s_trans_motor_spd_pid = {0};
PID_AbsoluteType s_trans_motor_pos_pid = {0};
PID_AbsoluteType s_fric_l_motor_pid = {0};
PID_AbsoluteType s_fric_r_motor_pid = {0};

motorData_t s_fric_l_motor = {0};
motorData_t s_fric_r_motor = {0};

/********************************** Static function *************************************/
void Shoot_PID_Reset(void)
{
	PID_AbsParam_Init(&s_trans_motor_pos_pid, p, i, d, 3000.0f, S1);
	PID_AbsParam_Init(&s_trans_motor_spd_pid, P, I, D, 3000.0f, 15000.0f);
}

/********************************** Interface function *************************************/
void Shoot_Init(void)
{
	s_trans_motor.turnMove = s_trans_motor.position_angle;
	memset(&s_trans_motor,  0, sizeof(motorData_t));//对电机信息结构体进行清零初始化
	memset(&s_fric_l_motor, 0, sizeof(motorData_t));//对电机信息结构体进行清零初始化
	memset(&s_fric_r_motor, 0, sizeof(motorData_t));//对电机信息结构体进行清零初始化

  PID_AbsParam_Init(&s_fric_l_motor_pid, 11, 1, 1, 3000.0f, 8000.0f);
  PID_AbsParam_Init(&s_fric_r_motor_pid, 11, 1, 0, 3000.0f, 8000.0f);
	PID_AbsParam_Init(&s_trans_motor_spd_pid, TRANS_MOTOR_SPEED_PID_KP, \
									TRANS_MOTOR_SPEED_PID_KI, TRANS_MOTOR_SPEED_PID_KD, \
									TRANS_MOTOR_SPEED_PID_ERRILIM, TRANS_MOTOR_SPEED_PID_MAXOUT);
	PID_AbsParam_Init(&s_trans_motor_pos_pid, TRANS_MOTOR_POS_PID_KP, \
                    TRANS_MOTOR_POS_PID_KI, TRANS_MOTOR_POS_PID_KD, \
                    TRANS_MOTOR_POS_PID_ERRILIM, TRANS_MOTOR_POS_PID_MAXOUT);

	laser_off();
	
}

void Shooting(void)
{
	if(((s_rcData.rc.s[1]==1) && (s_rcData.rc.s[0]==1)))
	{
		count++;
		s_trans_motor.coolingheat_limit = 34400;
	}
	else count = 0;

	if(fabs((double)s_trans_motor.turnMove - (double)s_trans_motor.position_angle) > 15.0f)
		roboSet.shootOnce = 0;

	if(count>shootFreq) count=shootFreq;

	if(count==shootFreq || roboSet.shootOnce==1)
	{
		if( fabs((double)s_trans_motor.turnMove - (double)s_trans_motor.position_angle) > 1.1*STEP)
			{
				s_trans_motor.turnMove = s_trans_motor.turnMove+STEP;
			}
		else
		{
			if((s_trans_motor.coolingheat_limit - BULL_HEAT) >= s_trans_motor.coolingheat)
				s_trans_motor.turnMove = s_trans_motor.turnMove-STEP;
		}
		count = 0;
		roboSet.shootOnce = 0;
	}

	switch(roboSet.e_shootMode)
	{
		case S_NORMAL:
    {
			laser_on();
			if(s_fric_l_motor.SpeedTarget>15.0f)
      {
				friSped_l = motor_speed_loop_pid_control(&s_fric_l_motor_pid, s_fric_l_motor.esc_back_speed,-4000);
				friSped_r = motor_speed_loop_pid_control(&s_fric_r_motor_pid, s_fric_r_motor.esc_back_speed, 4000);
			}
			else
			{
				friSped_l = motor_speed_loop_pid_control(&s_fric_l_motor_pid, s_fric_l_motor.esc_back_speed,-2660);
				friSped_r = motor_speed_loop_pid_control(&s_fric_r_motor_pid, s_fric_r_motor.esc_back_speed, 2660);
			}
			transOut = motor_cascade_loop_pid_control(&s_trans_motor_pos_pid, \
													&s_trans_motor_spd_pid, \
													s_trans_motor.position_angle, \
													s_trans_motor.turnMove, \
													s_trans_motor.esc_back_speed);
    } break;
		case S_STOP:
		{
			laser_off();
      friSped_l = motor_speed_loop_pid_control(&s_fric_l_motor_pid, s_fric_l_motor.esc_back_speed, 0);
			friSped_r = motor_speed_loop_pid_control(&s_fric_r_motor_pid, s_fric_r_motor.esc_back_speed, 0);
			s_trans_motor.turnMove = s_trans_motor.position_angle;
			transOut = motor_cascade_loop_pid_control(&s_trans_motor_pos_pid, \
													&s_trans_motor_spd_pid, \
													s_trans_motor.position_angle, \
													s_trans_motor.turnMove, \
													s_trans_motor.esc_back_speed);
		}	break;
		case S_NULL:
		{
			laser_off();
			friSped_l = 0;
			friSped_r = 0;
			transOut = 0;
		} break;
	}
	//电机输出
	CANTx_SendCurrent(&hcan2, 0x200, 0, transOut, friSped_l, friSped_r);
}

